﻿/*
    Copyright 2010, 2011 Eric Wong
	contact@optimalcycling.com
	http://www.optimalcycling.com
	Optimal Cycling - Advanced power pacing program for cyclists
	
	This file is part of Optimal Cycling.

    Optimal Cycling is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Optimal Cycling is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with Optimal Cycling.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef __OPENCL_VERSION__
	#include "Model.h"
#endif

// Comment out to disable separate wheel drag modelling
#define WHEEL_DRAG_SEPARATE

// Bearing Resistance Type
//#define BEARING_RESISTANCE_NONE
#define BEARING_RESISTANCE_CARTRIDGE_BEARING_DAHN

#define INTERPOLATE_VARIABLES

#define MODELACCEL_CURRPOS (*iData).s1
#define MODELACCEL_CURRSPEED (*iData).s2
#define MODELACCEL_CURRACCEL (*iData).s3

inline int ModelAcceleration(real4* iData,		// iTime {s0}, iPosition {s1}, iSpeed {s2}, iAccel {s3}
		real* iPower, 
		__global const real* powerVal,
		__global const real4* coursePosPoints, 
		__global const real* coursePosLinear,
		const real coursePosLinearMax,
		__global const real4* coursePosDirection,
		__global const real4* coursePosInfo,
		__global const real4* riderProp,
		__global const real4* wind, 
		__global const real4* climate,
		const sizeT numSimPoints,

		__global const real* dragCoeffWheelYawAngles,
		__global const real2* dragCoeffWheels, 
		const sizeT dragCoeffWheelsNumPts,

		__global const ModelInfo* modelInfo)
{
	sizeT left, right;
	real valDiff, controlVal;
	real currPower, currGrade, currAirDensity, currRollingResistanceCoeff, currHalfGeneralCDA,
		currWeightTotal, currInverseMassAndRotInertiaTotal;

	real4 currWind, currDirection;
	real4 currVelocityNeg;
	real4 relativeWindVelocity;
	real relativeWindMag;
	real relativeWindAngleCos;
	real relativeWindAngle;
	int relativeWindAngleCosFact;		// < 0 is a tail wind

	#ifdef WHEEL_DRAG_SEPARATE
		real2 currDragCoeffWheels;
		#ifdef __OPENCL_VERSION__
			real2 controlValVec2;
		#endif	/* __OPENCL_VERSION__ */
	#endif

	if (MODELACCEL_CURRSPEED < modelInfo->minSpeed)
		return 0;

	InterpolateBinaryParams(MODELACCEL_CURRPOS, coursePosLinear, numSimPoints, &left, &right, &valDiff, &controlVal);	

#if defined (INTERPOLATE_VARIABLES)
	{
		if (valDiff < SMALL_REAL)		// Use left for the index
		{
			currPower = powerVal[left];
			currGrade = coursePosInfo[left].s0;
			currRollingResistanceCoeff = coursePosInfo[left].s1;
			currAirDensity = climate[left].s0;

			if (modelInfo->stillWind)	
			{
			}
			else
			{
				currWind = wind[left];
				currDirection = coursePosDirection[left];			
			}

			#if defined (CONSTANT_GENERAL_CDA)
				currHalfGeneralCDA = modelInfo->halfCdA;
			#else
				currHalfGeneralCDA = riderProp[left].s0;
			#endif

			#if defined (CONSTANT_WEIGHT_AND_INERTIA)
				currWeightTotal = modelInfo->weightTotal;
				currInverseMassAndRotInertiaTotal = modelInfo->inverseMassAndRotInertiaTotal;
			#else
				currWeightTotal = riderProp[left].s1;
				currInverseMassAndRotInertiaTotal = riderProp[left].s2;
			#endif
		}
		else	// Between values
		{
			currPower = mix(powerVal[left], powerVal[right], controlVal);
			currGrade = coursePosInfo[left].s0;		// step type
			currRollingResistanceCoeff = mix(coursePosInfo[left].s1, coursePosInfo[right].s1, controlVal);
			currAirDensity = mix(climate[left].s0, climate[right].s0, controlVal);		

			if (modelInfo->stillWind)
			{
			}
			else
			{
				#ifdef __OPENCL_VERSION__
					real4 controlValVec4 = {controlVal, controlVal, controlVal, controlVal};
					currWind = mix(wind[left], wind[right], controlValVec4);
				#else
					currWind.s0 = mix(wind[left].s0, wind[right].s0, controlVal);
					currWind.s1 = mix(wind[left].s1, wind[right].s1, controlVal);
					currWind.s2 = mix(wind[left].s2, wind[right].s2, controlVal);
					currWind.s3 = mix(wind[left].s3, wind[right].s3, controlVal);
				#endif	/* __OPENCL_VERSION__ */
				
				currDirection = coursePosDirection[left];		// step type
			}

			#if defined (CONSTANT_GENERAL_CDA)
				currHalfGeneralCDA = modelInfo->halfCdA;
			#else
				currHalfGeneralCDA = mix(riderProp[left].s0, riderProp[right].s0, controlVal);
			#endif

			#if defined (CONSTANT_WEIGHT_AND_INERTIA)
				currWeightTotal = modelInfo->weightTotal;
				currInverseMassAndRotInertiaTotal = modelInfo->inverseMassAndRotInertiaTotal;
			#else
				currWeightTotal = mix(riderProp[left].s1, riderProp[right].s1, controlVal);
				currInverseMassAndRotInertiaTotal = mix(riderProp[left].s2, riderProp[right].s2, controlVal);
			#endif
		}

	}
#else

	currPower = mix(powerVal[left], powerVal[right], controlVal);
	currGrade = coursePosInfo[left].s0;
	currRollingResistanceCoeff = coursePosInfo[left].s1;
	currAirDensity = climate[left].s0;

	if (modelInfo->stillWind)	
	{
	}
	else
	{
		currWind = wind[left];
		currDirection = coursePosDirection[left];			
	}

	currHalfGeneralCDA = riderProp[left].s0;
	currWeightTotal = riderProp[left].s1;
	currInverseMassAndRotInertiaTotal = riderProp[left].s2;

#endif		// INTERPOLATE_VARIABLES


	*iPower = currPower;

	if (modelInfo->stillWind)
	{
		relativeWindMag = MODELACCEL_CURRSPEED;
		relativeWindAngleCos = 1;
		#ifdef WHEEL_DRAG_SEPARATE
			relativeWindAngle = 0;
		#endif
		relativeWindAngleCosFact = -1;
	}
	else
	{
		#ifdef __OPENCL_VERSION__
			currVelocityNeg = (-MODELACCEL_CURRSPEED) * currDirection;
			relativeWindVelocity = currVelocityNeg + currWind;
		#else
			currVelocityNeg.s0 = (-MODELACCEL_CURRSPEED) * currDirection.s0;
			currVelocityNeg.s1 = (-MODELACCEL_CURRSPEED) * currDirection.s1;
			currVelocityNeg.s2 = (-MODELACCEL_CURRSPEED) * currDirection.s2;
			currVelocityNeg.s3 = (-MODELACCEL_CURRSPEED) * currDirection.s3;

			relativeWindVelocity.s0 = currVelocityNeg.s0 + currWind.s0;
			relativeWindVelocity.s1	= currVelocityNeg.s1 + currWind.s1; 
			relativeWindVelocity.s2 = currVelocityNeg.s2 + currWind.s2;
			relativeWindVelocity.s3 = currVelocityNeg.s3 + currWind.s3;
		#endif	/* __OPENCL_VERSION__ */

		relativeWindMag = real4_length(relativeWindVelocity);			
		relativeWindAngleCos = clamp(real4_dot(real4_normalize(relativeWindVelocity), real4_normalize(currVelocityNeg)), NEG_ONE, ONE);
		relativeWindAngle = acos(relativeWindAngleCos);		// acos valid from [-Pi, Pi]
		relativeWindAngleCosFact = (relativeWindAngleCos < 0 ? 1 : -1);		// < 0 is a tail wind

	}

	#ifdef WHEEL_DRAG_SEPARATE
		// Wheel drag interpolation
		InterpolateBinaryParams(relativeWindAngle, dragCoeffWheelYawAngles, dragCoeffWheelsNumPts, &left, &right, &valDiff, &controlVal);
		if (valDiff < SMALL_REAL)		// Use left for the index
		{
			currDragCoeffWheels = dragCoeffWheels[left];
		}
		else	// Between values
		{
			#ifdef __OPENCL_VERSION__
				controlValVec2.s0 = controlVal;
				controlValVec2.s1 = controlVal;
				currDragCoeffWheels = mix(dragCoeffWheels[left], dragCoeffWheels[right], controlValVec2);
			#else
				currDragCoeffWheels.s0 = mix(dragCoeffWheels[left].s0, dragCoeffWheels[right].s0, controlVal);
				currDragCoeffWheels.s1 = mix(dragCoeffWheels[left].s1, dragCoeffWheels[right].s1, controlVal);
			#endif		// __OPENCL_VERSION__
		}

		// currDragCoeffWheels and currDragCoeffWheels are pre-multiplied with diameter and shielding factor
		MODELACCEL_CURRACCEL = ((currAirDensity * pown(relativeWindMag * relativeWindAngleCos, 2) * ( (currHalfGeneralCDA * relativeWindAngleCosFact) 
			+ (EIGTH_PI * relativeWindAngleCosFact) * 
			(currDragCoeffWheels.s0 + currDragCoeffWheels.s1)) )
			-(currWeightTotal * real_rsqrt(pown(currGrade, 2) + 1) * (currGrade +  currRollingResistanceCoeff))
			+ real_divide(currPower * modelInfo->driveTrainEfficiency, MODELACCEL_CURRSPEED)

				#ifdef BEARING_RESISTANCE_CARTRIDGE_BEARING_DAHN
					- (91.0f + 8.7f * MODELACCEL_CURRSPEED) * 0.001f
				#else
					
				#endif		

			) * (currInverseMassAndRotInertiaTotal);
	#else
		MODELACCEL_CURRACCEL = ((currAirDensity * pown(relativeWindMag * relativeWindAngleCos, 2) * ( (currHalfGeneralCDA * relativeWindAngleCosFact)))
			-(currWeightTotal * real_rsqrt(pown(currGrade, 2) + 1) * (currGrade +  currRollingResistanceCoeff))
			+ real_divide(currPower * modelInfo->driveTrainEfficiency, MODELACCEL_CURRSPEED)

				#ifdef BEARING_RESISTANCE_CARTRIDGE_BEARING_DAHN
					- (91.0f + 8.7f * MODELACCEL_CURRSPEED) * 0.001f
				#else
					
				#endif		

			) * (currInverseMassAndRotInertiaTotal);
	#endif
		
	return 1;
}


